#include "MBCameraView.h"
#include "MBMatrix3x3.h"
#include "MBDirectoryMethods.h"
#include "MBStringMethods.h"

#include <iostream>
#include <vector>

using std::cout;
using std::endl;
using std::string;
using std::vector;

const MBuint X = 0u;
const MBuint Y = 1u;
const MBuint Z = 2u;

const double RAD_TO_DEGREE = 180.0 / 3.14159265358979323846;

const MBMatrix4x4 RSP_TO_GL_COORD_SYSTEM( 1.0,  0.0,  0.0,  0.0,
										  0.0, -1.0,  0.0,  0.0,
										  0.0,  0.0, -1.0,  0.0,
										  0.0,  0.0,  0.0,  1.0 );

MBVec2 MBCameraView::cameraViewShiftVector = MBVec2( 0.0, 0.0 );

/**
  *	Constructs a new MBCameraView object with the passed arguments. All the passed
  * matrices have the same meaning as in a RiScan Pro project.
  *
  * @param imagePath full path of the image, the camera view belongs to
  * @param cameraModel const pointer to the underlying camera model
  * @param cameraPosID unique ID of the camera position
  * @param mount 4x4 matrix describing the transformation from the SOCS (Scanner's Own Coordinate System) into the CMCS (CaMera Coordinate System) at the reference orientation
  * @param COP 4x4 matrix holding the information about the orientation of the camera within the scanner's own coordinate system with respect to a reference orientation
  * @param SOP 4x4 matrix describing the transformation from the SOCS (Scanner's Own Coordinate System) into the PRCS (PRoject Coordinate System)
  * @param POP 4x4 matrix describing the transformation from the PRCS (PRoject Coordinate System) into the GLCS (GLobal Coordinate System)
  */
MBCameraView::MBCameraView(	const std::string &imagePath,
				const MBCameraModel *cameraModel,
				const MBuint &cameraPosID,
				const MBMatrix4x4 &mount,
				const MBMatrix4x4 &COP,
				const MBMatrix4x4 &SOP,
 				const MBMatrix4x4 &POP,
				const double usedImageAreaRadius )  :
 	mImagePath( imagePath ),
 	mCameraModel( cameraModel ),
	mCameraPosID( cameraPosID ),
	mUsedImageAreaRadius( usedImageAreaRadius )
{
	// result_matrix is the World Space to View Space transformation
	MBMatrix4x4 result_matrix = RSP_TO_GL_COORD_SYSTEM * mount * COP.GetInverse() * SOP.GetInverse() * POP.GetInverse();

	MBMatrix4x4 result_inverse = result_matrix.GetInverse();
	MBMatrix4x4 result_inverse_rot = result_inverse;
	
	// Eliminate the translation
	result_inverse_rot.At( 0u, 3u ) = 0.0;
	result_inverse_rot.At( 1u, 3u ) = 0.0;
	result_inverse_rot.At( 2u, 3u ) = 0.0;

	MBVec4 position = result_inverse * MBVec4( 0.0, 0.0, 0.0, 1.0 );
	MBVec4 up_vector = result_inverse_rot * MBVec4( 0.0, 1.0, 0.0, 1.0 );
	MBVec4 look_at = result_inverse_rot * MBVec4( 0.0, 0.0, -1.0, 1.0 );

	look_at[X] += position[X];
	look_at[Y] += position[Y];
	look_at[Z] += position[Z];

	mPosition[X] = position[X];
	mPosition[Y] = position[Y];
	mPosition[Z] = position[Z];

// 	if (MB::StringEndsWith( imagePath, "110908_081112_Image001.jpg" ))
// 	{
// 		int a = 0;
// 
// 		double m00 = result_matrix.At(0, 0);
// 		double m01 = result_matrix.At(0, 1);
// 		double m02 = result_matrix.At(0, 2);
// 
// 		double m10 = result_matrix.At(1, 0);
// 		double m11 = result_matrix.At(1, 1);
// 		double m12 = result_matrix.At(1, 2);
// 
// 		double m20 = result_matrix.At(2, 0);
// 		double m21 = result_matrix.At(2, 1);
// 		double m22 = result_matrix.At(2, 2);
// 
// 		double angle = acos(( m00 + m11 + m22 - 1)/2);
// 		double value = sqrt((m21 - m12) * (m21 - m12) +(m02 - m20) * (m02 - m20) + (m10 - m01) * (m10 - m01));
// 		double x = (m21 - m12) / value;
// 		double y = (m02 - m20) / value;
// 		double z = (m10 - m01) / value;
// 
// 		result_matrix.Print();
// 
// 		cout << x << " " << y << " " << z << " " << angle << endl;
// 
// 		angle = RAD_TO_DEGREE * 2.0 * atan( mCameraModel->GetIntrinsicParameters().mImageHeightInPixels / (2.0 * abs( mCameraModel->GetInternalCalibrationParameters().mFocalLengthYInPixels )) );
// 
// 		cout << "angle of view: " << angle << endl;
// 	}

	double angle = RAD_TO_DEGREE * 2.0 * atan( mCameraModel->GetIntrinsicParameters().mImageHeightInPixels / (2.0 * abs( mCameraModel->GetInternalCalibrationParameters().mFocalLengthYInPixels )) );
	double ratio = static_cast<double>( mCameraModel->GetIntrinsicParameters().mImageWidthInPixels ) / static_cast<double>( mCameraModel->GetIntrinsicParameters().mImageHeightInPixels );
	
	// Near distance set to 1 mm
	const double NEAR_DISTANCE = 0.001;
	// Far distance set to 100 km
	const double FAR_DISTANCE = 100000.0;

	mViewFrustum.SetCamInternals( angle, ratio, NEAR_DISTANCE, FAR_DISTANCE );

	mViewFrustum.SetCamDef(
		MBVec3( position[X], position[Y], position[Z] ),
		MBVec3( look_at[X], look_at[Y], look_at[Z] ),
		MBVec3( up_vector[X], up_vector[Y], up_vector[Z] ) );

	mWorldSpaceToViewSpaceTranslationVector[X] = result_matrix.At( 0, 3 );
	mWorldSpaceToViewSpaceTranslationVector[Y] = result_matrix.At( 1, 3 );
	mWorldSpaceToViewSpaceTranslationVector[Z] = result_matrix.At( 2, 3 );

	cv::Vec3d rotation;
	cv::Rodrigues(  MBMatrix3x3( result_matrix.At( 0, 0 ), result_matrix.At( 0, 1 ), result_matrix.At( 0, 2 ), 
								 result_matrix.At( 1, 0 ), result_matrix.At( 1, 1 ), result_matrix.At( 1, 2 ), 
								 result_matrix.At( 2, 0 ), result_matrix.At( 2, 1 ), result_matrix.At( 2, 2 ) ).mMatrix,
					rotation );

	mWorldSpaceToViewSpaceRotationVector = MBVec3( &rotation[X] );

	mWorldSpaceToViewSpaceTransformation = result_matrix;

	mMaskPath = std::string();
}

/**
  *	Constructs a new MBCameraView object with the passed arguments. This is the
  * constructor needed for AdamTech projects
  *
  * @param imagePath full path of the image, the camera view belongs to
  * @param cameraModel const pointer to the underlying camera model
  * @param cameraPosID unique ID of the camera position
  * @param locationX x-coordinate of the camera in 3D space
  * @param locationY y-coordinate of the camera in 3D space
  * @param locationZ z-coordinate of the camera in 3D space
  * @param rotationMatrix 3x3 matrix describing the world space to view space rotation of the camera
  */
MBCameraView::MBCameraView(	const string &imagePath,
		const MBCameraModel *cameraModel,
		const MBuint &cameraPosID,
		const double &locationX,
		const double &locationY,
		const double &locationZ,
		const MBMatrix3x3 &rotationMatrix,
		const double usedImageAreaRadius ) :
		mImagePath( imagePath ),
		mCameraModel( cameraModel ),
		mCameraPosID( cameraPosID ),
		mUsedImageAreaRadius( usedImageAreaRadius )
{
	// Inverse Translation (from world space to view space)
	MBMatrix4x4 translation = MBMatrix4x4::TranslationXYZ( -locationX, -locationY, -locationZ );

	// The rotation in the Adamtech ORI files is already the inverse transformation
	// (from world space to view space)
	MBMatrix4x4 rotation( rotationMatrix.At( 0, 0 ), rotationMatrix.At( 0, 1 ), rotationMatrix.At( 0, 2 ), 0.0,
		rotationMatrix.At( 1, 0 ), rotationMatrix.At( 1, 1 ), rotationMatrix.At( 1, 2 ), 0.0,
		rotationMatrix.At( 2, 0 ), rotationMatrix.At( 2, 1 ), rotationMatrix.At( 2, 2 ), 0.0,
		0.0, 0.0, 0.0, 1.0 );

	//////////
	// TEST
// 	double _angle = acos( (rotation.At( 0, 0 ) + rotation.At( 1, 1 ) + rotation.At( 2, 2 ) - 1.0) * 0.5 );
// 	double term = sqrt( pow( rotation.At( 2, 1 ) - rotation.At( 1, 2 ), 2.0 ) + pow( rotation.At( 0, 2 ) - rotation.At( 2, 0 ), 2.0 ) + pow( rotation.At( 1, 0 ) - rotation.At( 0, 1 ), 2.0 ) );
// 	double x = (rotation.At( 2, 1 ) - rotation.At( 1, 2 )) / term;
// 	double y = (rotation.At( 0, 2 ) - rotation.At( 2, 0 )) / term;
// 	double z = (rotation.At( 1, 0 ) - rotation.At( 0, 1 )) / term;
	//////////

	// result_matrix is the World Space to View Space Transformation
	MBMatrix4x4 result_matrix = rotation * translation;
	MBMatrix4x4 result_inverse = result_matrix.GetInverse();
	MBMatrix4x4 result_inverse_rot = result_inverse;

	// Eliminate the translation
	result_inverse_rot.At( 0u, 3u ) = 0.0;
	result_inverse_rot.At( 1u, 3u ) = 0.0;
	result_inverse_rot.At( 2u, 3u ) = 0.0;

	MBVec4 position = result_inverse * MBVec4( 0.0, 0.0, 0.0, 1.0 );
	MBVec4 up_vector = result_inverse_rot * MBVec4( 0.0, 1.0, 0.0, 1.0 );
	MBVec4 look_at = result_inverse_rot * MBVec4( 0.0, 0.0, -1.0, 1.0 );

	look_at[X] += position[X];
	look_at[Y] += position[Y];
	look_at[Z] += position[Z];

	mPosition[X] = position[X];
	mPosition[Y] = position[Y];
	mPosition[Z] = position[Z];

	double angle = RAD_TO_DEGREE * 2.0 * atan( mCameraModel->GetIntrinsicParameters().mImageHeightInPixels / (2.0 * abs( mCameraModel->GetInternalCalibrationParameters().mFocalLengthYInPixels )) );
	double ratio = static_cast<double>( mCameraModel->GetIntrinsicParameters().mImageWidthInPixels ) / static_cast<double>( mCameraModel->GetIntrinsicParameters().mImageHeightInPixels );

	// Near distance set to 1 mm
	const double NEAR_DISTANCE = 0.001;
	// Far distance set to 100 km
	const double FAR_DISTANCE = 100000.0;

	mViewFrustum.SetCamInternals( angle, ratio, NEAR_DISTANCE, FAR_DISTANCE );

	mViewFrustum.SetCamDef(
		MBVec3( position[X], position[Y], position[Z] ),
		MBVec3( look_at[X], look_at[Y], look_at[Z] ),
		MBVec3( up_vector[X], up_vector[Y], up_vector[Z] ) );

	mWorldSpaceToViewSpaceTranslationVector[0] = result_matrix.At( 0, 3 );
	mWorldSpaceToViewSpaceTranslationVector[1] = result_matrix.At( 1, 3 );
	mWorldSpaceToViewSpaceTranslationVector[2] = result_matrix.At( 2, 3 );

	cv::Vec3d cv_rotation;
	cv::Rodrigues(  MBMatrix3x3( result_matrix.At( 0, 0 ), result_matrix.At( 0, 1 ), result_matrix.At( 0, 2 ), 
		result_matrix.At( 1, 0 ), result_matrix.At( 1, 1 ), result_matrix.At( 1, 2 ), 
		result_matrix.At( 2, 0 ), result_matrix.At( 2, 1 ), result_matrix.At( 2, 2 ) ).mMatrix,
		cv_rotation );

	mWorldSpaceToViewSpaceRotationVector = MBVec3( &cv_rotation[X] );

	mWorldSpaceToViewSpaceTransformation = result_matrix;

	mMaskPath = std::string();
}

/**
  *	Constructs a new MBCameraView object with the passed arguments.
  *
  * @param imagePath full path of the image, the camera view belongs to
  * @param cameraModel const pointer to the underlying camera model
  * @param cameraPosID unique ID of the camera position
  * @param locationX x-coordinate of the camera in 3D space
  * @param locationY y-coordinate of the camera in 3D space
  * @param locationZ z-coordinate of the camera in 3D space
  * @param rotationX rotation around the x-axis inside an XYZ Euler rotation
  * @param rotationY rotation around the y-axis inside an XYZ Euler rotation
  * @param rotationZ rotation around the z-axis inside an XYZ Euler rotation
  */
MBCameraView::MBCameraView(	const std::string &imagePath,
				const MBCameraModel *cameraModel,
				const MBuint &cameraPosID,
				const double &locationX,
				const double &locationY,
				const double &locationZ,
				const double &rotationX,
				const double &rotationY,
				const double &rotationZ,
				const double usedImageAreaRadius ) :
	mImagePath( imagePath ),
	mCameraModel( cameraModel ),
	mCameraPosID( cameraPosID ),
	mUsedImageAreaRadius( usedImageAreaRadius )
{
	MBMatrix4x4 translation = MBMatrix4x4::TranslationXYZ( -locationX, -locationY, -locationZ );
	MBMatrix4x4 rotation = MBMatrix4x4::RotationXYZ( rotationX, rotationY, rotationZ ).GetTranspose();

	// result_matrix is the World Space to View Space Transformation
	MBMatrix4x4 result_matrix = rotation * translation;
	MBMatrix4x4 result_inverse = result_matrix.GetInverse();
	MBMatrix4x4 result_inverse_rot = result_inverse;

	// Eliminate the translation
	result_inverse_rot.At( 0u, 3u ) = 0.0;
	result_inverse_rot.At( 1u, 3u ) = 0.0;
	result_inverse_rot.At( 2u, 3u ) = 0.0;

	MBVec4 position = result_inverse * MBVec4( 0.0, 0.0, 0.0, 1.0 );
	MBVec4 up_vector = result_inverse_rot * MBVec4( 0.0, 1.0, 0.0, 1.0 );
	MBVec4 look_at = result_inverse_rot * MBVec4( 0.0, 0.0, -1.0, 1.0 );

	look_at[X] += position[X];
	look_at[Y] += position[Y];
	look_at[Z] += position[Z];

	mPosition[X] = position[X];
	mPosition[Y] = position[Y];
	mPosition[Z] = position[Z];

	double angle = RAD_TO_DEGREE * 2.0 * atan( mCameraModel->GetIntrinsicParameters().mImageHeightInPixels / (2.0 * abs( mCameraModel->GetInternalCalibrationParameters().mFocalLengthYInPixels )) );
	double ratio = static_cast<double>( mCameraModel->GetIntrinsicParameters().mImageWidthInPixels ) / static_cast<double>( mCameraModel->GetIntrinsicParameters().mImageHeightInPixels );

	// Near distance set to 1 mm
	const double NEAR_DISTANCE = 0.001;
	// Far distance set to 100 km
	const double FAR_DISTANCE = 100000.0;

	mViewFrustum.SetCamInternals( angle, ratio, NEAR_DISTANCE, FAR_DISTANCE );

	mViewFrustum.SetCamDef(
		MBVec3( position[X], position[Y], position[Z] ),
		MBVec3( look_at[X], look_at[Y], look_at[Z] ),
		MBVec3( up_vector[X], up_vector[Y], up_vector[Z] ) );

	mWorldSpaceToViewSpaceTranslationVector[0] = result_matrix.At( 0, 3 );
	mWorldSpaceToViewSpaceTranslationVector[1] = result_matrix.At( 1, 3 );
	mWorldSpaceToViewSpaceTranslationVector[2] = result_matrix.At( 2, 3 );

	cv::Vec3d cv_rotation;
	cv::Rodrigues(  MBMatrix3x3( result_matrix.At( 0, 0 ), result_matrix.At( 0, 1 ), result_matrix.At( 0, 2 ), 
		result_matrix.At( 1, 0 ), result_matrix.At( 1, 1 ), result_matrix.At( 1, 2 ), 
		result_matrix.At( 2, 0 ), result_matrix.At( 2, 1 ), result_matrix.At( 2, 2 ) ).mMatrix,
		cv_rotation );

	mWorldSpaceToViewSpaceRotationVector = MBVec3( &cv_rotation[X] );

	mWorldSpaceToViewSpaceTransformation = result_matrix;

	mMaskPath = std::string();
}

/**
  *	Returns a const pointer to the underlying camera model object
  *
  * @return const pointer to the underlying camera model object
  */
const MBCameraModel *MBCameraView::GetCameraModel( void ) const
{
	return mCameraModel;
}

/**
  *	Returns the unique ID of the camera position
  *
  * @return unique ID of the camera position
  */
const MBuint &MBCameraView::GetCameraPosID( void ) const
{
	return mCameraPosID;
}

/**
  *	Returns the full path of the image the camera view belongs to
  *
  * @return full path of the image the camera view belongs to
  */
const std::string &MBCameraView::GetImagePath( void ) const
{
	return mImagePath;
}

/**
  *	Returns the position of the camera in 3D space
  *
  * @return position of the camera in 3D space
  */
const MBVec3 &MBCameraView::GetPosition( void ) const
{
	return mPosition;
}

/**
  *	Returns the texcoord shift vector in UV-space. It's always (0.0, 0.0) for a MBCameraView object.
  *
  * @return texcoord shift vector in UV-space
  */
const MBVec2 &MBCameraView::GetTexcoordShiftVector( void ) const
{
	return cameraViewShiftVector;
}

/**
  *	Returns the full path of the undistorted image corresponding to this
  * MBCameraView object
  *
  * @return full path of the undistorted image
  */
const string &MBCameraView::GetUndistortedImagePath( void ) const
{
	return mUndistortedImagePath;
}

/**
  *	Returns the radius of effectively used image area, 1.0 is the distance
  * from the midpoint to the top (or bottom) of the image
  *
  * @return radius of effectively used image area
  */
const double &MBCameraView::GetUsedImageAreaRadius( void ) const
{
	return mUsedImageAreaRadius;
}

/**
  *	Returns a const reference to the view frustum of this camera view
  *
  * @return const reference to the view frustum of this camera view
  */
const MBCameraViewFrustum &MBCameraView::GetViewFrustum( void ) const
{
	return mViewFrustum;
}

/**
  *	Returns a 4x4 matrix describing the transformation from world space to view space
  *
  * @return 4x4 matrix describing the transformation from world space to view space
  */
const MBMatrix4x4 &MBCameraView::GetWorldSpaceToViewSpaceTransformation( void ) const
{
	return mWorldSpaceToViewSpaceTransformation;
}

/**
  *	Returns a 3D vector describing the rotation of the camera. This vector is
  * the return value of the OpenCV Rodrigues function
  *
  * @return 3D vector describing the rotation of the camera
  */
const MBVec3 &MBCameraView::GetWorldSpaceToViewSpaceRotationVector( void ) const
{
	return mWorldSpaceToViewSpaceRotationVector;
}

/**
  *	Returns the translation vector of the camera from world space to view space
  *
  * @return translation vector of the camera from world space to view space
  */
const MBVec3 &MBCameraView::GetWorldSpaceToViewSpaceTranslationVector( void ) const
{
	return mWorldSpaceToViewSpaceTranslationVector;
}

void MBCameraView::SetUndistortedImagePath( const std::string &undistortedImagePath )
{
	mUndistortedImagePath = undistortedImagePath;
}

void MBCameraView::SetMaskPath( const std::string &maskPath )
{
	mMaskPath = maskPath;
}

const std::string &MBCameraView::GetMaskPath( void ) const
{
	return mMaskPath;
}